#include "navigation.h"

void nav_update(nav_state_t *x, const vector3_t *w, const vector3_t *a, fixed_t dt)
{
	vector3_t wc, ac, t1, t2;
	quaternion_t u, temp;
	fixed_t s, sin_s, cos_s;

	// update position
	x->p.x.val += fixed_mul(x->v.x, dt).val;
	x->p.y.val += fixed_mul(x->v.y, dt).val;
	x->p.z.val += fixed_mul(x->v.z, dt).val;

	// correct IMU data using estimated biases
	vector3_sub(w, &x->bw, &wc);
	vector3_sub(a, &x->ba, &ac);

	// correct acceleration for IMU offset from CG
	vector3_cross(&wc, &NAV_IMU_OFFSET, &t1);
	vector3_cross(&wc, &t1, &t2);
	vector3_sub(&ac, &t2, &t1);

	// transform acceleration from body frame to ENU frame and adjust for gravity
	quaternion_rotate_inv(&x->q, &t1, &ac);
	ac.z.val -= NAV_G.val;

	// update velocity
	x->v.x.val += fixed_mul(ac.x, dt).val;
	x->v.y.val += fixed_mul(ac.y, dt).val;
	x->v.z.val += fixed_mul(ac.z, dt).val;

	// compute quaternion exponential for orientation update
	wc.x.val = (fixed_mul(wc.x, dt).val >> 1);
	wc.y.val = (fixed_mul(wc.y, dt).val >> 1);
	wc.z.val = (fixed_mul(wc.z, dt).val >> 1);
	s = vector3_length(&wc);
	fixed_sin_cos(s, &sin_s, &u.w);
	u.v.x = fixed_mul(fixed_div(wc.x, s), sin_s);
	u.v.y = fixed_mul(fixed_div(wc.y, s), sin_s);
	u.v.z = fixed_mul(fixed_div(wc.z, s), sin_s);

	// update orientation
	quaternion_mul(&x->q, &u, &temp);
	x->q.w = temp.w;
	x->q.v.x = temp.v.x;
	x->q.v.y = temp.v.y;
	x->q.v.z = temp.v.z;
}
